#include "display.h"
#include "delay.h"
#include <pic.h>

void WriteLCDCtrl(char b) {
	LCDbus = b;
	RS = 0;
	//AdrPort = LCDadr;
	AdrBit1=0;
	AdrBit2=0;
	AdrBit3=0;
	DelayUs(40);
	AdrEnable = 1;
	DelayUs(40);
	AdrEnable=0;
	DelayUs(100);
}

void WriteLCDData(char c){

	LCDbus = c;
	RS = 1;
	//AdrPort = LCDadr;
	AdrBit1=0;
	AdrBit2=0;
	AdrBit3=0;
	DelayUs(20);
	AdrEnable = 1;
	DelayUs(20);
	AdrEnable=0;
	DelayUs(50);
}

void LCDInit(void) {
	DelayMs (50);
	WriteLCDCtrl(FUNCSET);
	DelayMs(5);
	WriteLCDCtrl(FUNCSET);
	DelayUs(110);
	WriteLCDCtrl(FUNCSET);
	DelayUs(40);
	WriteLCDCtrl(FUNCSET);
	DelayUs(40);
	WriteLCDCtrl(DSPOFF);
	DelayUs(40);
	WriteLCDCtrl(DSPCLR);
	DelayMs(2);
	WriteLCDCtrl(ENTMOD);
	DelayUs(40);
	WriteLCDCtrl(DSPON);
	DelayUs(40);
}

void SwitchOffDisplay() {
	WriteLCDCtrl(DSPOFF);
	DelayUs(40);
}

void LCDCursor(char r, char c) {
		char cur = 0x80+r*0x40+c;
		WriteLCDCtrl (cur);
}

/*
void Write_Hex(char b) {
	if((b>>4)>9) {
		WriteLCDData('A'+((b>>4)-10));
	} else {
		WriteLCDData('0'+(b>>4));
	}
	if((b&0x0f)>9) {
		WriteLCDData('A'+((b&0x0f)-10));
	} else {
		WriteLCDData('0'+(b&0x0f));
	}
}
*/

void WriteSpeed(unsigned int speed) {
  union {
    struct {
      unsigned int low; //2 bytes
      unsigned long high;  //4 bytes
    } lng;
    unsigned char byte[6];
  } cdec;

  static bit encountered_only_zeros;
  signed char c;
  unsigned char d;

  LCDCursor(0, 6);
  //at least it prints '0' if it is zero, and doesnt skip all leading zeros!
  if (speed==0) {
		WriteLCDData(' ');  
		WriteLCDData(' ');
		WriteLCDData('0');  
		WriteLCDData(',');  
		WriteLCDData('0'); 
		WriteLCDData('0'); 
    return;
  }

  LCDCursor(0, 6);

  //implement the technique above
  cdec.lng.low=speed;
  cdec.lng.high=0;  //12W / 2l

   for (c=0;c<16;c++) {

    for (d=2;d<6;d++) {
      if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
      if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
    }

    //shift hi:lo <<1
    d=cdec.byte[1];
    cdec.lng.low<<=1;
    cdec.lng.high<<=1; 
   
    if ( (d& 0x80)==0x80) cdec.byte[2]|=1;
  }

  //extract BCD and put to default output device
  encountered_only_zeros=TRUE;  //for skipping leading zeros

  for (c=8;c>=4;c--) { //was c=9!
    d=cdec.byte[c>>1];
    if ((c&1)==0) {
      d&=0x0F;
    }
    else {
      d&=0xF0;
      d>>=4;
    }
    //if (d>=8) d-=3;
    if (d!=0||c==6) encountered_only_zeros=FALSE;
    d+='0';	//convert to digits
    if (encountered_only_zeros==FALSE) {
      WriteLCDData(d);  
    } else {
		WriteLCDData(' '); 
	}
	if (c==6) {
		WriteLCDData(','); 
	}
  } //37 words
}


void WriteAvgSpeed(unsigned int speed) {
  union {
    struct {
      unsigned int low; //2 bytes
      unsigned long high;  //4 bytes
    } lng;
    unsigned char byte[6];
  } cdec;

  static bit encountered_only_zeros;
  signed char c;
  unsigned char d;

  LCDCursor(1, 5);
  //at least it prints '0' if it is zero, and doesnt skip all leading zeros!
  if (speed==0) {
		WriteLCDData(' ');  
		WriteLCDData(' ');
		WriteLCDData('0');  
		WriteLCDData(',');  
		WriteLCDData('0'); 
		WriteLCDData('0'); 
    return;
  }

  //implement the technique above
  cdec.lng.low=speed;
  cdec.lng.high=0;  //12W / 2l

   for (c=0;c<16;c++) {

    for (d=2;d<6;d++) {
      if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
      if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
    }

    //shift hi:lo <<1
    d=cdec.byte[1];
    cdec.lng.low<<=1;
    cdec.lng.high<<=1; 
   
    if ( (d& 0x80)==0x80) cdec.byte[2]|=1;
  }

  //extract BCD and put to default output device
  encountered_only_zeros=TRUE;  //for skipping leading zeros

  for (c=8;c>=4;c--) {
    d=cdec.byte[c>>1];
    if ((c&1)==0) {
      d&=0x0F;
    }
    else {
      d&=0xF0;
      d>>=4;
    }
    //if (d>=8) d-=3;
    if (d!=0||c==6) encountered_only_zeros=FALSE;
    d+='0';	//convert to digits
    if (encountered_only_zeros==FALSE) {
      WriteLCDData(d);  
    } else {
		WriteLCDData(' '); 
	}
	if (c==6) {
		WriteLCDData(','); 
	}
  } //37 words
}

void WriteSleep() {
	 LCDCursor(0, 0);
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('S');
	 WriteLCDData('l');
	 WriteLCDData('e');
	 WriteLCDData('e');
	 WriteLCDData('p');
	 WriteLCDData('!');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
	 WriteLCDData('*');
}

void WriteReset() {
	LCDCursor(1, 5);
	WriteLCDData('-');
	WriteLCDData('R');
	WriteLCDData('e');
	WriteLCDData('s');
	WriteLCDData('e');
	WriteLCDData('t');
	WriteLCDData('-');
}

void WriteKPH() {
	LCDCursor(0, 0);
	WriteLCDData('S');
	WriteLCDData('p');
	WriteLCDData('e');
	WriteLCDData('e');
	WriteLCDData('d');
	WriteLCDData(':');
    // 6 signs speed...
	LCDCursor(0, 12);
	WriteLCDData('k');
	WriteLCDData('m');
	WriteLCDData('/');
	WriteLCDData('h');
}

void WriteSet1(char type) {
	LCDCursor(1, 0);
	WriteLCDData('T');
	WriteLCDData('r');
	WriteLCDData('i');
	WriteLCDData('p');
	WriteLCDData(':');
    // 8 signs distance/time or 6 signs speed...
	if(type=='D') {
		LCDCursor(1, 14);
		WriteLCDData('k');
		WriteLCDData('m');
	} else if(type=='T') {
		LCDCursor(1, 14);
		WriteLCDData('h');
		WriteLCDData(' ');
	} else {
		LCDCursor(1, 11);
		WriteLCDData(0xFD);
		WriteLCDData('k');
		WriteLCDData('m');
		WriteLCDData('/');
		WriteLCDData('h');
	}	
}

void WriteSet2(char type) {
	LCDCursor(1, 0);
	WriteLCDData('A');
	WriteLCDData('l');
	WriteLCDData('l');
	WriteLCDData(':');
	WriteLCDData(' ');
    // 8 signs distance/time or 6 signs speed...
	if(type=='D') {
		LCDCursor(1, 14);
		WriteLCDData('k');
		WriteLCDData('m');
	} else if(type=='T') {
		LCDCursor(1, 14);
		WriteLCDData('h');
		WriteLCDData(' ');
	} else {
		LCDCursor(1, 11);
		WriteLCDData(0xFD);
		WriteLCDData('k');
		WriteLCDData('m');
		WriteLCDData('/');
		WriteLCDData('h');
	}	
}

void WritePower() {
	LCDCursor(1, 0);
	WriteLCDData('P');
	WriteLCDData('o');
	WriteLCDData('w');
	WriteLCDData('.');
	WriteLCDData(':');
    // 8 signs distance/time or 6 signs speed...
	LCDCursor(1, 11);
	WriteLCDData('W');
	WriteLCDData('a');
	WriteLCDData('t');
	WriteLCDData('t');
	WriteLCDData(' ');
}

void WriteEnergy() {
	LCDCursor(1, 0);
	WriteLCDData('E');
	WriteLCDData('n');
	WriteLCDData('e');
	WriteLCDData('r');
	WriteLCDData(':');
    // 8 signs distance/time or 6 signs speed...
	LCDCursor(1, 14);
	WriteLCDData('M');
	WriteLCDData('J');
}

void WriteSettings() {
	LCDCursor(1, 0);
	WriteLCDData('W');
	WriteLCDData('h');
	WriteLCDData('e');
	WriteLCDData('e');
	WriteLCDData('l');
	WriteLCDData('c');
	WriteLCDData('i');
	WriteLCDData('r');
	WriteLCDData('c');
	WriteLCDData(':');
	LCDCursor(1, 15);
	WriteLCDData('m');
}

void WriteSettings2() {
	LCDCursor(1, 0);
	WriteLCDData('W');
	WriteLCDData('e');
	WriteLCDData('i');
	WriteLCDData('g');
	WriteLCDData('h');
	WriteLCDData('t');
	WriteLCDData(':');
	WriteLCDData(' ');
	WriteLCDData(' ');
	WriteLCDData(' ');
	LCDCursor(1, 13);
	WriteLCDData('k');
	WriteLCDData('g');
	WriteLCDData(' ');
}

//*****
//writes an *long* decimal to the default output device 
//outputs decimals up to 7 characters (ie: 9,999,999)
//skips leading zeros
//advantages: fast
//by Shane Tolmie (c)2001 from http://www.workingtex.com/htpic

void WriteDistTime(unsigned long binary)
{
  union {
    struct {
      unsigned long low; //4 bytes
      unsigned long high;  //4 bytes
    } lng;
    unsigned char byte[8];
  } cdec;

  static bit encountered_only_zeros;
  signed char c;
  unsigned char d;

  /*  
	binary->bcd conversion method

  1. Add 3 to every BCD nibble that's >=5
  2. Shift left
  Repeat until binary portion is all 0000
  
  Example:  Convert $F9 to BCD (ie: 3 nibbles, containing 249)   

  0000 0000 0000 : 1111 1001
  0000 0000 0001 : 1111 0010  (shift left)
  0000 0000 0011 : 1110 0100  (shift left)
  0000 0000 0111 : 1100 1000  (shift left)
  0000 0000 1010 : 1100 1000  (add 3)
  0000 0001 0101 : 1001 0000  (shift left)
  0000 0001 1000 : 1001 0000  (add 3)
  0000 0011 0001 : 0010 0000  (shift left)
  0000 0110 0010 : 0100 0000  (shift left)
  0000 1001 0010 : 0100 0000  (add 3)
  0001 0010 0100 : 1000 0000  (shift left)
  0010 0100 1001 : 0000 0000  (shift left)
     2    4    9 = 249

  And the left now holds BCD 249.
  MUCH easier than messing with IDIV and FDIV!!!	
  
  This technique can be used for other mixed radix 
  conversions, the magic number 3 is half the difference 
  in the radix  (16-10)/2 = 3.

	//For BCD->ascii, as in the 16-bit conversion below,
	//any number >=8 must have 3 subtracted from it.
*/

  //at least it prints '0' if it is zero, and doesnt skip all leading zeros!
  LCDCursor(1, 5);
  if (binary==0) {
		WriteLCDData(' ');  
		WriteLCDData(' ');
		WriteLCDData(' ');  
		WriteLCDData(' ');
		WriteLCDData(' ');  
		WriteLCDData('0');  
		WriteLCDData(',');  
		WriteLCDData('0'); 
		WriteLCDData('0'); 
    return;
  }

  //implement the technique above
  cdec.lng.low=binary;
  cdec.lng.high=0;  //12W / 2l

/* ALT
  for (c=0;c<32;c++) {
    //shift hi:lo <<1
    d=cdec.byte[3];
    cdec.lng.low<<=1;
    cdec.lng.high<<=1;    

    if ( (d& 0x80)==0x80) cdec.byte[4]|=1;
      
    for (d=4;d<7;d++) {
      if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
      if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
    }
  } //65W
*/

for (c=0;c<32;c++) {
  for (d=4;d<7;d++) {
    if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
    if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
  }
  //shift hi:lo <<1
  d=cdec.byte[3];
  cdec.lng.low<<=1;
  cdec.lng.high<<=1;
  if ( (d& 0x80)==0x80) cdec.byte[4]|=1;
} //65W


  //extract BCD and put to default output device
  encountered_only_zeros=TRUE;  //for skipping leading zeros

  for (c=15;c>=8;c--) {
    d=cdec.byte[c>>1];
    if ((c&1)==0) {
      d&=0x0F;
    }
    else {
      d&=0xF0;
      d>>=4;
    }
    //if (d>=8) d-=3;
    if (d!=0 || c==10) encountered_only_zeros=FALSE;
    d+='0';	//convert to digits
    if (encountered_only_zeros==FALSE) {
      WriteLCDData(d);  
    } else {
		WriteLCDData(' '); 
	}
	if (c==10) {
		WriteLCDData(','); 
	}
  } //37 words

} //114 words

/* Settings funktions */

void updateSettingsData(unsigned int wheelCircumference){
  union {
    struct {
      unsigned int low; //2 bytes
      unsigned int high;  //4 bytes
    } lng;
    unsigned char byte[6];
  } cdec;

  signed char c;
  unsigned char d;

  LCDCursor(1, 10);
  //at least it prints '0' if it is zero, and doesnt skip all leading zeros!
  if (wheelCircumference==0) {
		WriteLCDData('0');  
		WriteLCDData(',');  
		WriteLCDData('0'); 
		WriteLCDData('0'); 
		WriteLCDData('0');
    return;
  }

  //implement the technique above
  cdec.lng.low=wheelCircumference;
  cdec.lng.high=0;  //12W / 2l

   for (c=0;c<16;c++) {

    for (d=2;d<4;d++) {
      if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
      if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
    }

    //shift hi:lo <<1
    d=cdec.byte[1];
    cdec.lng.low<<=1;
    cdec.lng.high<<=1; 
   
    if ( (d& 0x80)==0x80) cdec.byte[2]|=1;
  }

  //extract BCD and put to default output device
  for (c=7;c>=4;c--) {
    d=cdec.byte[c>>1];
    if ((c&1)==0) {
      d&=0x0F;
    }
    else {
      d&=0xF0;
      d>>=4;
    }
    d+='0';	//convert to digits
      WriteLCDData(d);  
	if (c==7) {
		WriteLCDData(','); 
	}
  } //37 words
}

void updateSettingsData2(unsigned char inWeight) {
  union {
    struct {
      unsigned char low; //1 bytes
      unsigned int high;  //2 bytes
    } lng;
    unsigned char byte[3];
  } cdec;

  static bit encountered_only_zeros;
  signed char c;
  unsigned char d;

  LCDCursor(1, 10);
  //at least it prints '0' if it is zero, and doesnt skip all leading zeros!
  if (inWeight==0) {
		WriteLCDData(' ');   
		WriteLCDData(' '); 
		WriteLCDData('0'); 
    return;
  }

  //implement the technique above
  cdec.lng.low=inWeight;
  cdec.lng.high=0;  //8W / 2l

   for (c=0;c<8;c++) {

    for (d=1;d<3;d++) {
      if ((cdec.byte[d]&0x0F)>=0x05) cdec.byte[d]+=0x3;
      if ((cdec.byte[d]&0xF0)>=0x50) cdec.byte[d]+=0x30;
    }

    //shift hi:lo <<1
    d=cdec.byte[0];
    cdec.lng.low<<=1;
    cdec.lng.high<<=1; 
   
    if ( (d& 0x80)==0x80) cdec.byte[1]|=1;
  }

  //extract BCD and put to default output device
  encountered_only_zeros=TRUE;  //for skipping leading zeros

  for (c=4;c>=2;c--) {
    d=cdec.byte[c>>1];
    if ((c&1)==0) {
      d&=0x0F;
    }
    else {
      d&=0xF0;
      d>>=4;
    }
    //if (d>=8) d-=3;
    if (d!=0) encountered_only_zeros=FALSE;
    d+='0';	//convert to digits
    if (encountered_only_zeros==FALSE) {
      WriteLCDData(d);  
    } else {
		WriteLCDData(' '); 
	}
  } //37 words
}


void setSettingsCursor(unsigned char digit) {
	if (digit==3) {
		digit++;
	} 
	LCDCursor(1, (14-digit));
}
/* replaced by macros
void enterSettings() {
	WriteLCDCtrl(DSP_CURSOR);
}
void leaveSettings(){
	WriteLCDCtrl(DSPON);
}
*/

void setSettingsCursor2(unsigned char digit) {
	LCDCursor(1, (12-digit));
}
